%author: wxj233
%time: 2023.10.25 12:00
%function: 主要用于完成特征点仿真以及轨迹模拟，整个场景在二维平面上进行

clc;
clear;
clear Function;
close all;

% 3倍sigma点就是精度
p_d = 0.4/3;  % 测距精度0.4m
p_v = 0.0277/3;  % 测速精度0.0277m/s
p_theta = 0.5/180*pi/3; % 测角精度0.3°（近距离），远距离是0.1°

% 初始化仿真器
% dt
simulater = Simulater(1);

% 产生第一个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster1_1 = simulater.generateFeaturePoints(10, 2.2, [0.9,0.9,0.9,0.9], 1);
cluster1_2 = simulater.generateFeaturePoints(10, 2.2, [0.9, 0.9], 2);
% 根据特征点群，产生第一条轨迹
% cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
[T1_1_1, ep1_1_1, ev1_1_1] = simulater.generateTrack(cluster1_1, [10, 0], [0, 1], [0.01, -0.005], 0, [0, 100], 1, p_d, p_v, p_theta, 10, 20);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
[T1_2_1, ep1_2_1, ev1_2_1] = simulater.generateTrack(cluster1_2, [10, 0], [0, 1], [0.01, -0.005], 0, [0, 100], 1, p_d, p_v, p_theta, 11, 21);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

[T1_1_2, ep1_1_2, ev1_1_2] = simulater.generateTrack(cluster1_1, ep1_1_1, ev1_1_1, [0.01, -0.01], 100, [1, 100], 1, p_d, p_v, p_theta, 10, 20);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
[T1_2_2, ep1_2_2, ev1_2_2] = simulater.generateTrack(cluster1_2, ep1_2_1, ev1_2_1, [0.01, -0.01], 100, [1, 100], 1, p_d, p_v, p_theta, 11, 21);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

% % 产生第二个目标特征点群
% % len, width, probabilitys, varargin{seed}
% cluster2 = simulater.generateFeaturePoints(5, 2.2, [0.5, 0.5, 0.5], 3);
% % % 根据特征点群，产生第二条轨迹
% % %cluster, point0, v, a, startTime, endTime, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
% T2 = simulater.generateTrack(cluster2, [90, 0], [0, 1], [-0.01, -0.005], [0, 200], 2, p_d, p_v, p_theta, 31, 32);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

% 产生第二个目标特征点群
% len, width, probabilitys, varargin{seed}
% cluster3 = simulater.generateFeaturePoints(5, 2.2, [0.9, 0.5, 0.5], 3);
% % 根据特征点群，产生第二条轨迹
% %cluster, point0, v, a, startTime, endTime, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
% T3 = simulater.generateTrack(cluster3, [0, 0], [0, 1], [0.015, -0.005], [0, 200], 1, p_d, p_v, p_theta, 41, 42);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]


% 引入噪声, 根据区域大小和噪声密度来算噪声数量，且数量服从泊松分布, 在区域内服从均匀分布，区域自己根据轨迹范围设定
% density varargin:{seed}
[noise, area] = simulater.generateNoise(0, 1); % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

% vMax, dr, dvr, dtheta, q, p, vAssDoor, inDoor, fplifeMax, tlifeMax, extR, amend
mfpTracer = MFPTracer(10, p_d*p_d, p_v*p_v, p_theta*p_theta, 0.01, 0.9, 0.95, 0.95, 10, 3, 5, 3);
[frame, isNextFrame]= simulater.getNextFrame();
frames = [];
while isNextFrame
    frame = mfpTracer.preproccess(frame, 8, 2, 1, 1, 10);  % 预处理points, epsilon, minpts, zoomX, zoomY, zoomV
    frames = [frames; frame];  % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]

    figure(1);
    gscatter(frames(:,10), frames(:,11), frames(:,12));hold on;  
%     gscatter(frame(:,10), frame(:,11), frame(:,12));hold on; 
    disp("当前第："+num2str(simulater.timeIndex)+"帧");  % 还未被追踪
    for track = mfpTracer.Tracks
        for fp = track.fps
            plot(fp.points(:,13), fp.points(:,15));hold on;
            te = "T"+num2str(fp.track.id)+" f:"+num2str(fp.id);
            text(fp.points(end,13), fp.points(end,15), te);
        end
    end
    hold off;
    axis(area(1:4));
    title("轨迹信息"+num2str(simulater.timeIndex));
    xlabel("x(m)");
    ylabel("y(m)");
    legend([]);
    grid on;
    if simulater.timeIndex == 30
        disp("pause");
    end
    
    mfpTracer.tracer(frame);
    [frame, isNextFrame]= simulater.getNextFrame();
end


